#include <ros/ros.h>
#include <stdio.h>
#include <stdlib.h>
#include <thread>
#include <vector>
#include "./sdk/include/celex5/celex5.h"
#include "./sdk/include/celextypes.h"
#include "sensor_msgs/Imu.h"
#include "std_msgs/Float64MultiArray.h"
#include "std_msgs/UInt32MultiArray.h"
#include "uav_event/CameraData.h"
#include "uav_event/Event.h"
#include "uav_event/EventVector.h"
#include "common_constants.h"

using namespace std;

/*
获取当前时间
@return:
    double 当前的时间戳（单位为秒）
*/
double time() {
    ros::Time rosTime = ros::Time::now();
    double res = rosTime.toSec();
    return res;
}

inline int ceil(int a, int b) {
    return a / b + (a % b ? 1 : 0);
}

// 定义暂存数据的单元
vector<uav_event::Event> eventBuffer;
bool eventBufferLock = false;
vector<sensor_msgs::Imu> imuBuffer;
bool imuBufferLock = false;

// 定义
ros::Publisher nodePub;
ros::Subscriber eventSub;
ros::Subscriber imuSub;

double maxInnerStamp = 0.0;
double eventStart = 0.0;
double eventEnd = 0.0;
double imuStart = 0.0;
double imuEnd = 0.0;
double eventLast = 0.0;
double imuLast = 0.0;
double currentInnerBase = 0.0;


void publish() {
    double msgTime = time();
    int lockCounter;
    uav_event::CameraData pubRes;
    std_msgs::UInt32MultiArray eventData;
    std_msgs::Float64MultiArray imuData;

    if (!eventBuffer.empty()) {
        lockCounter = 0;
        // while (eventBufferLock && lockCounter < 50) {
        //     printf("Event Locked!\n");
        //     lockCounter++;
        // }
        // eventBufferLock = true;
        vector<uav_event::Event> currentEvents = eventBuffer;
        eventBuffer.clear();
        // eventBufferLock = false;
        bool eventTimeRecorded = false;
        for (int i = 0; i < currentEvents.size(); i++) {
            uav_event::Event data = currentEvents.at(i);
            double currentInnerStamp = data.off_pixel_timestamp * 1e-6;
            maxInnerStamp = currentInnerStamp > maxInnerStamp
                                ? currentInnerStamp
                                : maxInnerStamp;
            if (currentInnerBase == 0.0) {
                currentInnerBase = msgTime - currentInnerStamp;
            }
            if (eventLast > currentInnerStamp &&
                eventLast > maxInnerStamp - 0.1 && currentInnerStamp < 0.1 &&
                eventLast - currentInnerStamp >= 5.0) {
                currentInnerBase += maxInnerStamp;
            }
            eventLast = currentInnerStamp;
            double currentTime = currentInnerBase + currentInnerStamp;
            if (!eventTimeRecorded) {
                eventStart = currentTime;
                eventTimeRecorded = true;
            }
            eventStart = currentTime < eventStart ? currentTime : eventStart;
            eventEnd = currentTime > eventEnd ? currentTime : eventEnd;
            unsigned int currentSec = (unsigned int)currentTime;
            unsigned int currentNSec =
                (unsigned int)((currentTime - currentSec) * 1000000000);
            unsigned int currentP = data.polarity > 0;
            unsigned int currentY = HEIGHT - 1 - data.x;
            unsigned int currentX = data.y;
            eventData.data.push_back(currentSec);
            eventData.data.push_back(currentNSec);
            eventData.data.push_back(currentP);
            eventData.data.push_back(currentY);
            eventData.data.push_back(currentX);
        }
    }

    if (!imuBuffer.empty()) {
        lockCounter = 0;
        // while (imuBufferLock && lockCounter < 50) {
        //     printf("IMU Locked!\n");
        //     lockCounter++;
        // }
        // imuBufferLock = true;
        vector<sensor_msgs::Imu> currentIMUs = imuBuffer;
        imuBuffer.clear();
        // imuBufferLock = false;
        bool imuTimeRecorded = false;
        for (int i = 0; i < currentIMUs.size(); i++) {
            sensor_msgs::Imu data = currentIMUs.at(i);
            double imuTimeStamp =
                data.header.stamp.sec + data.header.stamp.nsec * 1e-9;
            double imuOmegaZ = data.angular_velocity.z;
            double imuOmegaY = data.angular_velocity.y;
            double imuOmegaX = data.angular_velocity.x;
            if (!imuTimeRecorded) {
                imuStart = imuTimeStamp;
                imuTimeRecorded = true;
            }
            imuStart = imuTimeStamp < imuStart ? imuTimeStamp : imuStart;
            imuEnd = imuTimeStamp > imuEnd ? imuTimeStamp : imuEnd;
            imuData.data.push_back(imuTimeStamp);
            imuData.data.push_back(imuOmegaZ);
            imuData.data.push_back(imuOmegaY);
            imuData.data.push_back(imuOmegaX);
            imuLast = imuTimeStamp;
        }
    }

    int finalEventSize = eventData.data.size();
    int finalIMUSize = imuData.data.size();

    pubRes.header.stamp.sec = (int)msgTime;
    pubRes.header.stamp.nsec =
        (int)((msgTime - pubRes.header.stamp.sec) * NSEC);
    if (finalEventSize) {
        pubRes.events = eventData.data;
    }
    pubRes.eventcol.push_back("t_sec");
    pubRes.eventcol.push_back("t_nsec");
    pubRes.eventcol.push_back("p");
    pubRes.eventcol.push_back("y");
    pubRes.eventcol.push_back("x");
    if (finalIMUSize) {
        pubRes.imus = imuData.data;
    }
    pubRes.imucol.push_back("t");
    pubRes.imucol.push_back("angvel_z");
    pubRes.imucol.push_back("angvel_y");
    pubRes.imucol.push_back("angvel_x");
    nodePub.publish(pubRes);

    string startColor = finalEventSize ? "\033[36m" : "\033[35m";
    string formatStr = "Time %.6f, %d events, %d IMUs.\n";
    string endColor = finalEventSize ? "\033[0m" : "\033[0m";
    printf((startColor + formatStr + endColor).data(), msgTime, finalEventSize,
           finalIMUSize);
}

/*
事件的回调函数
@params:
    msg: uav_event::EventVector 收到的（多个事件）消息
*/
void eventCallback(const uav_event::EventVector& msg) {
    vector<uav_event::Event> newEvents = msg.events;
    unsigned int length = msg.vector_length;
    // int lockCounter = 0;
    // while (eventBufferLock && lockCounter < 50) {
    //     printf("Event Locked!\n");
    //     lockCounter++;
    // }
    // eventBufferLock = true;
    for (int i = 0; i < length; i++) {
        eventBuffer.push_back(newEvents.at(i));
    }
    // eventBufferLock = false;
    publish();
}

/*
IMU的回调函数
@params:
    msg: sensor_msgs::Imu 收到的IMU消息（角速度等）
*/
void imuCallback(const sensor_msgs::Imu& msg) {
    // int lockCounter = 0;
    // while (imuBufferLock && lockCounter < 50) {
    //     printf("IMU Locked!\n");
    //     lockCounter++;
    // }
    // imuBufferLock = true;
    imuBuffer.push_back(msg);
    // imuBufferLock = false;
}

/*
订阅者的线程函数，用于执行订阅者线程
*/
void subThreadFunc() {
    ros::NodeHandle n;
    eventSub =
        n.subscribe("/celex5_mipi/events", 1, eventCallback);  // 订阅事件消息
    imuSub = n.subscribe("/celex5_mipi/imu", 1, imuCallback);  // 订阅IMU消息
    ros::spin();  // 开始循环监听消息
}

/*
发布者的线程函数，用于发布本结点的执行结果
*/
void pubThreadFunc() {
    ros::NodeHandle n;
    nodePub = n.advertise<uav_event::CameraData>("/uav_event/event_imu_data", 1);
    ros::Rate frequency(USEC / DURATION_USEC);

    // while (ros::ok()) {
    //     publish();
    // }
}

int main(int argc, char** argv) {
    ros::init(argc, argv, "event_sub");  // 初始化ROS环境

    thread subThread(subThreadFunc);  // 定义订阅者线程
    thread pubThread(pubThreadFunc);  // 定义发布者线程

    subThread.join();  // 加入订阅者线程
    pubThread.join();  // 加入发布者线程

    return 0;
}